#include <pathplanner.h>
#include <defs.h>
#include <utils.h>
#include <io.h>
#include <user.h>
#include <printer.h>
#include <messages.h>
#include <iobuf.h>
#include <track.h>
#include <routefinder.h>
#include <controller.h>
#include <reserver.h>

void LandmarkNotifier() {
  landmark_event event;
  
  int storage_tid;
  Receive(&storage_tid, NULL, 0);
  Reply(storage_tid, NULL, 0);
  
  for (;;) {
    AwaitEvent(EVENT_LANDMARK, (char*)&event, sizeof(landmark_event));
    Send(storage_tid, (char*)&event, sizeof(landmark_event), NULL, 0);
  }
  Panic("LandmarkNotifier", "Chuck Norris!");
}

void LandmarkStorage() {

  landmark_event msg;
  
  iobuf bufTrain;
  IobufInit(&bufTrain);  
  
  iobuf bufLandmark;
  IobufInit(&bufLandmark);
  
  int request_waiting = FALSE;
  int has_path = FALSE;
  
  int notifier_tid = Create( 6, &LandmarkNotifier, "LandmarkNotifier" );
  
  int planner_tid;
  Receive(&planner_tid, NULL, 0);
  Reply(planner_tid, NULL, 0);
    
  Send(notifier_tid, NULL, 0, NULL, 0);
  
  int sender_tid;  
  for (;;) {
    Receive(&sender_tid, (char*)&msg, sizeof(landmark_event));
    if (sender_tid == notifier_tid) {
      Reply(notifier_tid, NULL, 0);
      // PrintAt(1, 24, "has_path: %d\r\n", has_path);
      if (!has_path) continue;      
      
      IobufEnqueue(&bufTrain, msg.train);
      IobufEnqueue(&bufLandmark, msg.landmark);
      
      /*
      char name2[5];
      InternalToExternal(msg.landmark, name2); 
      // PrintAt(1, 24, "enqueue: %s\r\n", name2);
      */
        
      if (request_waiting) {
        IobufDequeue(&bufTrain, &msg.train);          
        IobufDequeue(&bufLandmark, &msg.landmark);
        request_waiting = FALSE;
        Reply(planner_tid, (char*)&msg, sizeof(landmark_event));
      }
    }
    else if (sender_tid == planner_tid) {
      has_path = msg.has_path;
      request_waiting = TRUE;
      if (!IobufEmpty(&bufTrain)) {
        IobufDequeue(&bufTrain, &msg.train);
        IobufDequeue(&bufLandmark, &msg.landmark);
        request_waiting = FALSE;
        Reply(planner_tid, (char*)&msg, sizeof(landmark_event));
      }
    }
    else {
      Panic("LandmarkStorage", "Invalid sender inside LandmarkStorage.");
    }
  }
  Panic("LandmarkStorage", "Chuck Norris!");
}


/* ENGINE PATH PLANNER STUFF */
void FollowPath(engine_path *P, int storage_tid) {
  landmark_event event;
  int next = -1;
  int next_reverse = -2;
  iobuf buf;
  IobufInit(&buf);
  char name[5];
  char *dir;
  int skipped = FALSE;
  resource_event resource;
  
  if (NextExpectedLandmark(P->train) != P->path[0])
    Rv(P->train);
  while (TRUE) {
  
    next = P->path[P->path_index];
    
    InternalToExternal(next, name);
    PrintAt(1, 24, "next: %s\r\n", name);
    
    if (next == UNDEFINED) {
      Tr(P->train, 0);
      break;
    }
    
    int oneAhead = P->path[P->path_index + 1];
    int twoAhead = P->path[P->path_index + 2];    
    
    
    if (oneAhead != UNDEFINED && !ReserveTrack(next, oneAhead)) {
      Tr(P->train, 0);
      do {
        AwaitEvent(EVENT_FREE_RESOURCE, (char*)&resource, sizeof(resource_event));
      } while (!((resource.type == TRACK && resource.arg0 == next && resource.arg1 == oneAhead) ||
                  (resource.type == TRACK && resource.arg1 == next && resource.arg0 == oneAhead)));
    }
    if (P->path_index > 1)
      FreeTrack(P->path[P->path_index - 2], P->path[P->path_index - 1]);

    if (oneAhead != UNDEFINED && twoAhead != UNDEFINED) {
      if (SwitchCurved(oneAhead, twoAhead)) {       
        int switchAfter = SwitchAfterSwitch(oneAhead, dir);
        if (switchAfter) {
          Sw(InternalToSwitch(switchAfter), *dir);
        } 
        Sw(InternalToSwitch(oneAhead), 'C');

        if (ShouldReverse(next, oneAhead, twoAhead)) {
          InternalToExternal(oneAhead, name);
          PrintAt(1, 24, "SWC: next reverse on: %s\r\n", name);
        
          if (next_reverse == -2)
            next_reverse = oneAhead;
          else
            IobufEnqueue(&buf, oneAhead);
        }
      }
      else if (SwitchStraight(oneAhead, twoAhead)) {
        int switchAfter = SwitchAfterSwitch(oneAhead, dir);
        if (switchAfter) {
          Sw(InternalToSwitch(switchAfter), *dir);
        }
        Sw(InternalToSwitch(oneAhead), 'S');        
        
        if (ShouldReverse(next, oneAhead, twoAhead)) {
          InternalToExternal(oneAhead, name);
          PrintAt(1, 24, "SWS: next reverse on: %s\r\n", name);
        
          if (next_reverse == -2)
            next_reverse = oneAhead;
          else
            IobufEnqueue(&buf, oneAhead);
        }
      }
    }
    Tr(P->train, 5);
    
    if (!skipped) {     
      do {
        Send(storage_tid, P->has_path, sizeof(int), (char*)&event, sizeof(landmark_event));
        if (event.landmark == oneAhead) {
          skipped = TRUE;
          break;
        }
      } while (next != event.landmark);
    }
    else
      skipped = FALSE;
      
    InternalToExternal(event.landmark, name);
    PrintAt(1, 24, "landmark: %s\r\n", name);    
    
    if (next == next_reverse) {    
      InternalToExternal(next, name);
      PrintAt(1, 24, "reversing on: %s\r\n", name);
      if (!IobufEmpty(&buf)) {      
        InternalToExternal(oneAhead, name);
        PrintAt(1, 24, "R: next reverse on: %s\r\n", name);
        IobufDequeue(&buf, &next_reverse);
      }
      else
        next_reverse = -2;        
      
      Delay(1);
      Rv(P->train);
    }
    P->path_index++;
  } 
}

void PathPlanner() {
  engine_path P;
  path_req msg;
  
  int storage_tid = Create( 6, &LandmarkStorage, "LandmarkStorage" );

  int manager_tid;
  Receive(&manager_tid, (char*)&P.train, sizeof(int));
  Reply(manager_tid, NULL, 0);
  
  Send(storage_tid, NULL, 0, NULL, 0);

  // TODO: we need some kind of way to deal with multiple copies of this.
  // As it is, this will overwrite the existing entry and probably Panic.
  
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "path-planner-%d", P.train);
  RegisterAs(buf);  
  
  int sender_tid;
  for ( ; ; ) {    
    Receive(&sender_tid, (char*)&msg, sizeof(path_req));
    switch (msg.type) {
      case MSG_PATH_GOTO:
        if (P.train != msg.train)
          Panic("PathPlanner", "GOTO message to wrong PathPlanner");
        P.source = msg.source;
        P.dest = msg.target;
        P.has_path = FindRoute(P.source, P.dest, P.path);
        if (P.has_path) {
          P.path_index = 0;
          FollowPath(&P, storage_tid);
        }
        Reply(sender_tid, NULL, 0);
        break;
      default:
        Panic("PathPlanner", "Invalid message type");
        break;
    }
  }
  Panic("PathPlanner", "Chuck Norris!");
}

void SendTrainTo(int train, int source, int target) {
  char buf[MAX_NAME_LEN];
  SPrintf(buf, "path-planner-%d", train);
  int planner_tid = WhoIs(buf);
  path_req msg;
  msg.type = MSG_PATH_GOTO;
  msg.source = source;
  msg.target = target;
  msg.train = train;
  Send(planner_tid, (char*)&msg, sizeof(path_req), NULL, 0);
}
